#include "motion_controller.h"
#include "led_manager.h"
#include "led_control_center.h"
#include <math.h>

// ==================== 系统参数配置 ====================
// 🔧 优化：集中化参数管理，便于调试和维护
struct MotionConfig {
    // 电机参数配置
    int rotationStepsPerRev = 8200;     // 旋转轴步数/圈
    int radiusMaxSteps = 4400;          // 半径轴最大步数
    float compensationFactor = 0.25f;   // 补偿系数
    int maxSpeedSPS = 400;              // 最大速度(步/秒)
    
    // 归零相关参数
    int homingSpeedSPS = 400;           // 🔧 归零速度与绘图速度统一
    int homingStepSize = 300;            // 🔧 小步长，防止越过限位开关
    unsigned long homingTimeout = 180000; // 归零超时(3分钟)
    float maxHomingDegrees = 720.0f;    // 最大归零角度(2圈)
    float maxHomingDistance = 120.0f;   // 最大归零距离(mm)
};

// 全局配置实例
static MotionConfig g_config;

// 向后兼容的全局变量（保持现有接口）
int ROTATION_STEPS_PER_REV = g_config.rotationStepsPerRev;
int RADIUS_MAX_STEPS = g_config.radiusMaxSteps;
float COMPENSATION_FACTOR = g_config.compensationFactor;
int MAX_SPEED_SPS = g_config.maxSpeedSPS;

// ==================== 性能优化常量 ====================
// 🔧 优化：统一管理性能相关常量
namespace MotionConstants {
    // 调试输出控制
    const int DEBUG_UPDATE_INTERVAL = 1000;        // 基础调试信息间隔
    const int DEBUG_VERBOSE_INTERVAL = 5000;       // 详细调试信息间隔
    const int DEBUG_MINIMAL_INTERVAL = 10000;      // 最小调试信息间隔
    
    // LED和UI更新
    const int LED_UPDATE_INTERVAL = 50;            // LED更新间隔(ms)
    
    // 标定相关
    const int CALIBRATION_SPEED = 80;              // 标定专用低速
    
    // 运动控制精度
    const float POSITION_TOLERANCE = 0.1f;         // 位置容差
}

// 🔧 使用配置结构中的参数，移除重复定义

MotionController::MotionController(AccelStepper* rotMotor, AccelStepper* radMotor, int thetaHome, int rhoHome) {
    rotationMotor = rotMotor;
    radiusMotor = radMotor;
    
    // 将步进电机添加到MultiStepper
    steppers.addStepper(*rotationMotor);        
    steppers.addStepper(*radiusMotor);
    
    // 绘图相关初始化（保持不变）
    currentPosition = {0, 0, 0, 0};  // 初始化绘图逻辑位置
    targetPosition = {0, 0, 0, 0};   // 初始化目标位置
    isMoving = false;                // 初始化运动状态
    lastMoveTime = 0;               // 初始化最后移动时间
    sweepPaused = false;            // 初始化扫图暂停状态
    accumulatedCompensation = 0.0f; // 初始化累积补偿
    ledManager = nullptr;           // 初始化LED管理器指针
    ledControlCenter = nullptr;     // 初始化LED控制中心指针
    
    // === 新增：归零相关初始化（从stepper_controller移植） ===
    thetaHomePin = thetaHome;
    rhoHomePin = rhoHome;
    isHoming = false;
    thetaStatus = MOTOR_IDLE;
    rhoStatus = MOTOR_IDLE;
    
    // === 新增：电机管理初始化（从stepper_controller移植） ===
    motorsEnabled = false;
    lastMotorActivity = millis();
    motorIdleTimeout = 30000;        // 默认30秒空闲超时
    autoMotorManagement = true;      // 默认启用自动管理
    
    // === 新增：标定参数初始化（从stepper_controller移植） ===
    // 🔧 动态计算标定值：基于结构参数而非硬编码实测值
    updateCalibrationFromStructure();  // 根据结构参数计算标定值
    
    // === 新增：半径轴补偿初始化（从stepper_controller移植） ===
    radiusCompensationEnabled = true;
    radiusCompensationRatio = COMPENSATION_FACTOR;  // 🔧 统一使用COMPENSATION_FACTOR
}

void MotionController::init() {
    Serial.println("🔧 MotionController 初始化开始...");
    
    // 🔧 优化：使用配置参数设置电机
    rotationMotor->setMaxSpeed(g_config.maxSpeedSPS);
    rotationMotor->setAcceleration(g_config.maxSpeedSPS);
    radiusMotor->setMaxSpeed(g_config.maxSpeedSPS);
    radiusMotor->setAcceleration(g_config.maxSpeedSPS);
    
    // 🔧 优化：初始化位置状态
    rotationMotor->setCurrentPosition(0);
    radiusMotor->setCurrentPosition(0);
    currentPosition = {0, 0, 0, 0};
    targetPosition = {0, 0, 0, 0};
    
    // 更新极坐标（基于绘图逻辑位置）
    stepsToPolar(currentPosition.rotation_steps, currentPosition.radius_steps,
               currentPosition.theta, currentPosition.rho);
    
    // 🔧 优化：统一的初始化信息输出
    Serial.printf("📐 机械参数: 旋转=%d步/圈, 半径=%d最大步数\n", 
                  g_config.rotationStepsPerRev, g_config.radiusMaxSteps);
    Serial.printf("🚀 运动参数: 速度=%d步/秒, 补偿=%.3f\n", 
                  g_config.maxSpeedSPS, g_config.compensationFactor);
    Serial.printf("📍 初始位置: 逻辑[%ld,%ld] 物理[%ld,%ld]\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps,
                  rotationMotor->currentPosition(), radiusMotor->currentPosition());
    Serial.println("✅ MotionController 初始化完成");
}

void MotionController::moveTo(float theta, float rho) {
    // 🔧 优化：减少详细调试输出，提升性能
    static unsigned long lastDebugTime = 0;
    if (millis() - lastDebugTime > MotionConstants::DEBUG_UPDATE_INTERVAL) {
        Serial.printf("moveTo: θ=%.3f弧度, ρ=%.3f\n", theta, rho);
        lastDebugTime = millis();
    }
    
    long rotationSteps, radiusSteps;
    polarToSteps(theta, rho, rotationSteps, radiusSteps);
    
    moveToSteps(rotationSteps, radiusSteps);    
}

void MotionController::moveToSteps(long rotationSteps, long radiusSteps) {
    // 🔧 优化：减少冗余调试输出
    static unsigned long lastDetailedLog = 0;
    bool shouldLog = (millis() - lastDetailedLog > MotionConstants::DEBUG_UPDATE_INTERVAL);
    
    // 计算绘图逻辑增量
    long rotationDelta = rotationSteps - currentPosition.rotation_steps;
    long radiusDelta = radiusSteps - currentPosition.radius_steps;
    
    // 计算补偿值
    long compensationValue = calculateRadiusCompensation(rotationDelta);
    
    // 基于电机物理位置计算目标位置
    long physicalRotationTarget = rotationMotor->currentPosition() + rotationDelta;
    long physicalRadiusTarget = radiusMotor->currentPosition() + radiusDelta - compensationValue;
    
    // 🔧 优化：只在必要时输出详细日志
    if (shouldLog) {
        Serial.printf("移动: Δ[%ld,%ld] → 目标[%ld,%ld] 补偿=%ld\n", 
                      rotationDelta, radiusDelta, physicalRotationTarget, physicalRadiusTarget, compensationValue);
        lastDetailedLog = millis();
    }
    
    // 设置电机目标位置
    long positions[2] = {physicalRotationTarget, physicalRadiusTarget};
    steppers.moveTo(positions);
    
    // 更新状态
    targetPosition.rotation_steps = rotationSteps;
    targetPosition.radius_steps = radiusSteps;
    isMoving = true;
    lastMoveTime = millis();
    updateMotorActivity();
}

bool MotionController::update() {
    // 如果已暂停，直接返回true，不运行电机
    if (!isMoving) return true;
    
    // 运行MultiStepper - 这会同步移动两个电机
    bool isRunning = steppers.run();
    
    // 实时更新LED位置（按照附件验证方案）
    if (ledControlCenter) {
        // 基于当前电机物理位置计算实时极坐标
        float currentTheta, currentRho;
        stepsToPolar(rotationMotor->currentPosition(), radiusMotor->currentPosition(),
                    currentTheta, currentRho);
        ledControlCenter->updatePosition(currentTheta, currentRho);
    }
    
    // 🔧 优化：减少运行时调试输出频率
    static unsigned long lastStatusUpdate = 0;
    if (millis() - lastStatusUpdate > MotionConstants::DEBUG_VERBOSE_INTERVAL) { // 5秒输出一次
        Serial.printf("运行中: θ剩余=%ld, ρ剩余=%ld\n", 
                      rotationMotor->distanceToGo(), radiusMotor->distanceToGo());
        lastStatusUpdate = millis();
    }
    
    // 检查移动是否完成
    if (rotationMotor->distanceToGo() == 0 && radiusMotor->distanceToGo() == 0) {
        isMoving = false;
        
        // 更新绘图逻辑位置
        currentPosition.rotation_steps = targetPosition.rotation_steps;
        currentPosition.radius_steps = targetPosition.radius_steps;
        
        // 更新极坐标（基于绘图逻辑位置）
        stepsToPolar(currentPosition.rotation_steps, currentPosition.radius_steps,
                    currentPosition.theta, currentPosition.rho);
        
        // 🔧 优化：大幅减少完成信息输出频率，防止绘图时串口过载
        static unsigned long lastCompletionLog = 0;
        if (millis() - lastCompletionLog > MotionConstants::DEBUG_MINIMAL_INTERVAL) {  // 每10秒输出一次
            Serial.printf("移动完成: 逻辑[%ld,%ld] 物理[%ld,%ld]\n", 
                          currentPosition.rotation_steps, currentPosition.radius_steps,
                          rotationMotor->currentPosition(), radiusMotor->currentPosition());
            lastCompletionLog = millis();
        }
        
        return true; // 移动完成
    }
    
    return false; // 还在移动中
}

void MotionController::stop() {
    Serial.println("🛑 强制暂停运动控制器");
    
    // 立即设置暂停标志
    isMoving = false;
    sweepPaused = true;  // 同时设置扫图暂停标志
    
    Serial.println("🔄 扫图暂停标志已设置，扫图将在下次检查时停止");
    
    // 停止电机运动
    rotationMotor->stop();
    radiusMotor->stop();
    
    // 🚨 关键修复：不要破坏绘图逻辑位置！
    // 错误的做法：把电机物理位置赋给绘图逻辑位置，会破坏补偿累积
    // currentPosition.rotation_steps = rotationMotor->currentPosition();  // ❌ 删除！
    // currentPosition.radius_steps = radiusMotor->currentPosition();      // ❌ 删除！
    
    // 正确做法：保持绘图逻辑位置不变，只记录物理停止位置用于调试
    long stopRotationPos = rotationMotor->currentPosition();
    long stopRadiusPos = radiusMotor->currentPosition();
    
    // 更新极坐标（基于绘图逻辑位置，不是物理位置）
    stepsToPolar(currentPosition.rotation_steps, currentPosition.radius_steps,
                currentPosition.theta, currentPosition.rho);

    // 🔧 重要修复：为了确保停止后的位置同步正确，需要小心处理两种位置
    Serial.printf("🛑 停止时位置状态: 绘图逻辑=[%ld,%ld], 电机物理=[%ld,%ld]\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps,
                  stopRotationPos, stopRadiusPos);
    
    // 🎯 关键：让currentPosition保持绘图逻辑状态，不要与物理位置混淆
    // 这样可以保证start()函数能正确计算剩余距离
    Serial.printf("✅ 绘图逻辑位置保持: 旋转=%ld步, 半径=%ld步\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("📍 电机物理停止位置: 旋转=%ld步, 半径=%ld步\n", 
                  stopRotationPos, stopRadiusPos);
    Serial.printf("🎯 位置差异: 旋转差=%ld步, 半径差=%ld步\n", 
                  stopRotationPos - currentPosition.rotation_steps,
                  stopRadiusPos - currentPosition.radius_steps);
    
    // 设置MultiStepper的目标位置为当前物理位置，确保不会有意外移动
    long stopPositions[2];
    stopPositions[0] = stopRotationPos;
    stopPositions[1] = stopRadiusPos;
    steppers.moveTo(stopPositions);
    
    Serial.println("🎯 运动控制器暂停完成");
    Serial.println("💡 使用 'start' 继续未完成的移动");
    Serial.println("💡 绘图逻辑位置已保留，确保无缝恢复");
}

void MotionController::start() {
    Serial.println("运动控制器准备就绪");
    Serial.printf("当前位置: 旋转=%ld步, 半径=%ld步\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("目标位置: 旋转=%ld步, 半径=%ld步\n", 
                  targetPosition.rotation_steps, targetPosition.radius_steps);
    
    // 重置扫图暂停标志，允许扫图继续
    sweepPaused = false;
    Serial.println("🔄 扫图暂停标志已重置，扫图可以继续执行");
    
    // 不修改任何位置状态，保持流式模式的连续性
    // 真正的重置只在 resetToZero() 中进行
    Serial.println("运动控制器已准备，等待绘制指令...");
}

void MotionController::polarToSteps(float theta, float rho, long& rotationSteps, long& radiusSteps) {
    // 计算目标角度对应的步数
    rotationSteps = (long)((theta * ROTATION_STEPS_PER_REV) / (2 * PI));
    
    // 半径处理
    radiusSteps = (long)(rho * RADIUS_MAX_STEPS);

}

void MotionController::stepsToPolar(long rotationSteps, long radiusSteps, float& theta, float& rho) {
    // 步数转换为角度 - 反向转换匹配反转的方向
    theta = (float)(rotationSteps * 2 * PI) / ROTATION_STEPS_PER_REV;
    
    // 步数转换为半径比例 - 反向转换匹配反转的方向
    rho = (float)radiusSteps / RADIUS_MAX_STEPS;
}

long MotionController::calculateRadiusCompensation(long rotationStepsDelta) {
    // 🎯 精确补偿计算：累积浮点补偿，防止舍入误差累积
    
    // 累积精确的浮点补偿
    float exactCompensation = rotationStepsDelta * COMPENSATION_FACTOR;
    accumulatedCompensation += exactCompensation;
    
    // 四舍五入到最近整数作为实际补偿
    long actualCompensation = (long)round(accumulatedCompensation);
    
    // 减去已应用的补偿，保留小数部分用于下次累积
    accumulatedCompensation -= actualCompensation;
    
    // 🔧 优化：减少调试输出频率，防止绘图时串口过载导致死机
    static unsigned long lastCompensationLog = 0;
    if (millis() - lastCompensationLog > MotionConstants::DEBUG_VERBOSE_INTERVAL) {  // 每5秒输出一次
        Serial.printf("🎯 精确补偿: 增量=%ld步, 理论补偿=%.3f, 实际补偿=%ld, 累积余数=%.3f\n", 
                      rotationStepsDelta, exactCompensation, actualCompensation, accumulatedCompensation);
        lastCompensationLog = millis();
    }
    
    return actualCompensation;
}

void MotionController::resetCompensation() {
    accumulatedCompensation = 0.0f;
    Serial.println("🔄 累积补偿已重置为零");
}

Position MotionController::getCurrentPosition() {
    return currentPosition;
}

bool MotionController::isMovingNow() {
    // 🔧 修复：更准确的运动状态检查
    if (!isMoving) return false;
    
    // 检查是否还有剩余距离需要移动
    bool hasDistance = (rotationMotor->distanceToGo() != 0 || radiusMotor->distanceToGo() != 0);
    
    // 如果没有剩余距离，但刚设置了目标（100ms内），仍然认为在移动中
    if (!hasDistance && (millis() - lastMoveTime < 100)) {
        return true;
    }
    
    return hasDistance;
}

void MotionController::setCompensationFactor(float factor) {
    COMPENSATION_FACTOR = factor;
    radiusCompensationRatio = factor;  // 🔧 同步更新归零补偿参数
    Serial.printf("✅ 补偿系数设置为: %.3f (绘图和归零统一)\n", factor);
}

void MotionController::setMaxSpeed(float stepsPerSecond) {
    MAX_SPEED_SPS = (int)stepsPerSecond;
    rotationMotor->setMaxSpeed(stepsPerSecond);
    radiusMotor->setMaxSpeed(stepsPerSecond);
    Serial.printf("🚀 最大速度统一设置为: %.0f步/秒 (影响start/move/home所有命令)\n", stepsPerSecond);
}

void MotionController::setAcceleration(float stepsPerSecondPerSecond) {
    rotationMotor->setAcceleration(stepsPerSecondPerSecond);
    radiusMotor->setAcceleration(stepsPerSecondPerSecond);
    Serial.printf("加速度设置为: %.0f步/秒²\n", stepsPerSecondPerSecond);
}

void MotionController::setRotationStepsPerRev(int steps) {
    ROTATION_STEPS_PER_REV = steps;
    // 🔧 重要：更新标定值以反映新的机械参数
    updateCalibrationFromStructure();
    Serial.printf("🔧 旋转轴每圈步数设置为: %d步，标定值已同步更新\n", steps);
}

void MotionController::setRadiusMaxSteps(int steps) {
    RADIUS_MAX_STEPS = steps;
    // 🔧 重要：更新标定值以反映新的机械参数
    updateCalibrationFromStructure();
    Serial.printf("🔧 半径轴最大步数设置为: %d步，标定值已同步更新\n", steps);
}

void MotionController::printStatus() {
    Serial.println("=== MotionController 状态 ===");
    
    // 计算目标位置的详细角度信息
    float targetDegrees = targetPosition.theta * 180.0 / PI;
    int targetCircles = (int)(targetDegrees / 360.0);
    float targetRemainder = fmod(targetDegrees, 360.0);
    if (targetRemainder < 0) {
        targetRemainder += 360.0;
        targetCircles -= 1;
    }
    
    // 计算当前位置的详细角度信息
    float currentDegrees = currentPosition.theta * 180.0 / PI;
    int currentCircles = (int)(currentDegrees / 360.0);
    float currentRemainder = fmod(currentDegrees, 360.0);
    if (currentRemainder < 0) {
        currentRemainder += 360.0;
        currentCircles -= 1;
    }
    
    // 显示目标位置
    if (targetCircles == 0) {
        Serial.printf("目标位置: 角度=%.3f弧度(%.1f度), 半径=%.3f\n", 
                      targetPosition.theta, targetDegrees, targetPosition.rho);
    } else {
        Serial.printf("目标位置: 角度=%.3f弧度(%.1f度=%d圈+%.1f度), 半径=%.3f\n", 
                      targetPosition.theta, targetDegrees, targetCircles, targetRemainder, targetPosition.rho);
    }
    
    // 显示当前位置
    if (currentCircles == 0) {
        Serial.printf("当前位置: 角度=%.3f弧度(%.1f度), 半径=%.3f\n", 
                      currentPosition.theta, currentDegrees, currentPosition.rho);
    } else {
        Serial.printf("当前位置: 角度=%.3f弧度(%.1f度=%d圈+%.1f度), 半径=%.3f\n", 
                      currentPosition.theta, currentDegrees, currentCircles, currentRemainder, currentPosition.rho);
    }
    
    Serial.printf("步数位置: 旋转=%ld步, 半径=%ld步\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("移动状态: %s\n", isMoving ? "运动中" : "静止");
    Serial.printf("配置参数: %d步/圈, %d最大半径步数, %.3f补偿系数\n", 
                  ROTATION_STEPS_PER_REV, RADIUS_MAX_STEPS, COMPENSATION_FACTOR);
    Serial.println("===========================");
}

void MotionController::printPosition() {
    // 计算详细的角度信息
    float actualDegrees = currentPosition.theta * 180.0 / PI;
    
    // 计算完整圈数和余角
    int fullCircles = (int)(actualDegrees / 360.0);
    float remainderDegrees = fmod(actualDegrees, 360.0);
    if (remainderDegrees < 0) {
        remainderDegrees += 360.0;
        fullCircles -= 1;
    }
    
    if (fullCircles == 0) {
        // 没有完整圈数时，简单显示
        Serial.printf("当前位置: 角度=%.3f弧度(%.1f度), 半径=%.3f, 步数=[%ld,%ld]\n", 
                      currentPosition.theta, 
                      actualDegrees,
                      currentPosition.rho, 
                      currentPosition.rotation_steps, 
                      currentPosition.radius_steps);
    } else {
        // 有完整圈数时，详细显示
        Serial.printf("当前位置: 角度=%.3f弧度(%.1f度=%d圈+%.1f度), 半径=%.3f, 步数=[%ld,%ld]\n", 
                      currentPosition.theta, 
                      actualDegrees,
                      fullCircles,
                      remainderDegrees,
                      currentPosition.rho, 
                      currentPosition.rotation_steps, 
                      currentPosition.radius_steps);
    }
}

void MotionController::syncPosition() {
    // 错误的做法：把电机物理位置赋给绘图逻辑位置！
    // currentPosition.rotation_steps = rotationMotor->currentPosition();  // 已禁用
    // currentPosition.radius_steps = radiusMotor->currentPosition();      // 已禁用
    
    // 正确的做法：绘图逻辑位置应该保持独立
    // 绘图逻辑位置只能通过targetPosition来更新，不能直接从电机物理位置同步
    
    Serial.println("syncPosition已禁用：绘图逻辑位置与电机物理位置必须分离！");
    Serial.printf("绘图逻辑位置: 旋转=%ld步, 半径=%ld步 (相对原点距离)\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("电机物理位置: 旋转=%ld步, 半径=%ld步 (包含补偿累积)\n", 
                  rotationMotor->currentPosition(), radiusMotor->currentPosition());
}

void MotionController::resetToZero() {
    Serial.println("强制重置MotionController到零位...");
    
    // 停止所有运动
    isMoving = false;
    rotationMotor->stop();
    radiusMotor->stop();
    
    // 强制设置电机位置为零
    rotationMotor->setCurrentPosition(0);
    radiusMotor->setCurrentPosition(0);
    
    // 重置所有内部位置状态
    currentPosition = {0, 0, 0, 0};
    targetPosition = {0, 0, 0, 0};
    
    // 重置累积补偿
    resetCompensation();
    
    // 设置MultiStepper目标位置为零
    long zeroPositions[2] = {0, 0};
    steppers.moveTo(zeroPositions);
    
    Serial.println("MotionController已强制重置到零位");
    Serial.printf("当前位置: 旋转=%ld步, 半径=%ld步\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("电机位置: 旋转=%ld步, 半径=%ld步\n", 
                  rotationMotor->currentPosition(), radiusMotor->currentPosition());
}

void MotionController::forceResetFromHoming() {
    Serial.println("🔄 从StepperController归零后强制同步MotionController...");
    
    // 停止所有运动
    isMoving = false;
    
    // 🚨 关键修复：强制重置电机物理位置为零
    Serial.println("🔧 强制重置电机物理位置到零位...");
    rotationMotor->setCurrentPosition(0);
    radiusMotor->setCurrentPosition(0);
    
    // 强制重置所有内部位置状态为零
    currentPosition = {0, 0, 0, 0};
    targetPosition = {0, 0, 0, 0};
    
    // 重置累积补偿
    resetCompensation();
    
    // 设置MultiStepper目标位置为零
    long zeroPositions[2] = {0, 0};
    steppers.moveTo(zeroPositions);
    
    Serial.println("✅ MotionController已从归零操作完全同步重置");
    Serial.printf("✨ 绘图逻辑位置: 旋转=%ld步, 半径=%ld步\n", 
                  currentPosition.rotation_steps, currentPosition.radius_steps);
    Serial.printf("🔧 电机物理位置: 旋转=%ld步, 半径=%ld步\n", 
                  rotationMotor->currentPosition(), radiusMotor->currentPosition());
    Serial.printf("🎯 极坐标位置: 角度=%.3f弧度, 半径=%.3f\n", 
                  currentPosition.theta, currentPosition.rho);
    Serial.println("🎉 归零后位置完全同步，准备开始新的绘图任务");
}

void MotionController::setLEDManager(LEDManager* manager) {
    ledManager = manager;
    Serial.println("✅ MotionController已连接LEDManager - 支持实时位置更新和归零LED效果");
}

void MotionController::setLEDControlCenter(LEDControlCenter* center) {
    ledControlCenter = center;
    Serial.println("✅ MotionController已连接LEDControlCenter - 统一LED管理架构");
}

// ==================== 电机管理功能（从stepper_controller移植） ====================

void MotionController::enableMotors() {
    // 重新配置引脚为输出模式（防止disable后引脚状态异常）
    // 注意：这里假设使用28BYJ-48，实际引脚在init()中已配置
    
    rotationMotor->enableOutputs();
    radiusMotor->enableOutputs();
    motorsEnabled = true;
    lastMotorActivity = millis();
    Serial.println("🔋 电机已使能 - 输出已启用");
}

void MotionController::disableMotors() {
    // 停止电机并释放力矩
    rotationMotor->stop();
    radiusMotor->stop();
    
    rotationMotor->disableOutputs();
    radiusMotor->disableOutputs();
    
    motorsEnabled = false;
    thetaStatus = MOTOR_IDLE;
    rhoStatus = MOTOR_IDLE;
    Serial.println("🔋 电机已完全禁用 - 进入节能模式");
}

bool MotionController::areMotorsEnabled() {
    return motorsEnabled;
}

void MotionController::configure28BYJ48() {
    // 28BYJ-48高速配置 - 统一参数管理
    rotationMotor->setMaxSpeed(MAX_SPEED_SPS);
    rotationMotor->setAcceleration(MAX_SPEED_SPS);
    radiusMotor->setMaxSpeed(MAX_SPEED_SPS);
    radiusMotor->setAcceleration(MAX_SPEED_SPS);
    
    Serial.printf("28BYJ-48配置 - 统一速度: %d步/秒, 加速度: %d\n", MAX_SPEED_SPS, MAX_SPEED_SPS);
}

// ==================== 电机自动管理功能（从stepper_controller移植） ====================

void MotionController::enableAutoMotorManagement(bool enable) {
    autoMotorManagement = enable;
    if (enable) {
        Serial.println("✅ 电机自动管理已启用 - 空闲时将自动断电节能");
    } else {
        Serial.println("❌ 电机自动管理已禁用 - 电机将保持常开状态");
    }
}

void MotionController::setMotorIdleTimeout(unsigned long timeoutMs) {
    motorIdleTimeout = timeoutMs;
    Serial.printf("⏰ 电机空闲超时设置为: %lu秒\n", timeoutMs / 1000);
}

void MotionController::updateMotorActivity() {
    lastMotorActivity = millis();
    
    // 如果启用了自动管理且电机当前未使能，则自动使能
    if (autoMotorManagement && !motorsEnabled) {
        enableMotors();
        Serial.println("🔋 检测到电机活动，自动使能电机");
    }
}

void MotionController::checkMotorIdle() {
    if (!autoMotorManagement || !motorsEnabled) {
        return;  // 未启用自动管理或电机已禁用
    }
    
    // 保护机制：绘图和回零过程中不能自动断电
    if (isHoming || isMoving) {
        updateMotorActivity();  // 活动过程中持续更新活动时间
        return;
    }
    
    if (thetaStatus == MOTOR_HOMING || rhoStatus == MOTOR_HOMING || 
        thetaStatus == MOTOR_MOVING || rhoStatus == MOTOR_MOVING) {
        updateMotorActivity();  // 电机运行过程中持续更新活动时间
        return;
    }
    
    // 检查是否有电机正在运行（双重保险）
    bool thetaMoving = (rotationMotor->distanceToGo() != 0);
    bool rhoMoving = (radiusMotor->distanceToGo() != 0);
    bool anyMotorMoving = thetaMoving || rhoMoving;
    
    if (anyMotorMoving) {
        updateMotorActivity();
        return;
    }
    
    // 检查是否超过空闲时间
    unsigned long idleTime = millis() - lastMotorActivity;
    if (idleTime > motorIdleTimeout) {
        Serial.printf("💤 电机空闲 %.1f秒，自动断电节能\n", idleTime / 1000.0);
        disableMotors();
    }
}

bool MotionController::isAutoMotorManagementEnabled() const {
    return autoMotorManagement;
}

unsigned long MotionController::getMotorIdleTimeout() const {
    return motorIdleTimeout;
}

unsigned long MotionController::getLastMotorActivity() const {
    return lastMotorActivity;
}

// ==================== 状态查询功能（从stepper_controller移植） ====================

MotorStatus MotionController::getThetaStatus() {
    return thetaStatus;
}

MotorStatus MotionController::getRhoStatus() {
    return rhoStatus;
}

const char* MotionController::getMotorStatusString(MotorStatus status) {
    switch (status) {
        case MOTOR_IDLE:   return "空闲";
        case MOTOR_MOVING: return "运动中";
        case MOTOR_HOMING: return "回零中";
        case MOTOR_ERROR:  return "错误";
        default:           return "未知";
    }
}

void MotionController::printMotorStatus() {
    Serial.println("=== 电机管理状态 ===");
    Serial.println("=== 28BYJ-48电机状态 ===");
    
    // 基本电机状态
    Serial.printf("🔋 电机使能: %s\n", motorsEnabled ? "✅已启用" : "❌已禁用");
    Serial.printf("🤖 自动管理: %s\n", autoMotorManagement ? "✅启用" : "❌禁用");
    Serial.printf("⚙️  半径补偿: %s", radiusCompensationEnabled ? "✅启用" : "❌禁用");
    if (radiusCompensationEnabled) {
        Serial.printf(" (比例: %.3f)", radiusCompensationRatio);
    }
    Serial.println();
    
    if (autoMotorManagement) {
        unsigned long idleTime = (millis() - lastMotorActivity) / 1000;
        unsigned long timeoutSec = motorIdleTimeout / 1000;
        Serial.printf("⏱️  空闲时间: %lu秒 (超时: %lu秒)\n", idleTime, timeoutSec);
    }
    
    // 角度电机状态
    Serial.printf("🔄 角度电机(θ): %ld步 - %s\n", 
        rotationMotor->currentPosition(), getMotorStatusString(thetaStatus));
    if (rotationMotor->distanceToGo() != 0) {
        Serial.printf("   目标距离: %ld步\n", rotationMotor->distanceToGo());
    }
    
    // 半径电机状态  
    Serial.printf("📏 半径电机(ρ): %ld步 - %s\n", 
        radiusMotor->currentPosition(), getMotorStatusString(rhoStatus));
    if (radiusMotor->distanceToGo() != 0) {
        Serial.printf("   目标距离: %ld步\n", radiusMotor->distanceToGo());
    }
    
    // 整体状态
    if (isHoming) {
        Serial.println("🏠 系统状态: 回零操作中");
    } else if (thetaStatus == MOTOR_MOVING || rhoStatus == MOTOR_MOVING) {
        Serial.println("▶️  系统状态: 绘图运行中");
    } else if (thetaStatus == MOTOR_ERROR || rhoStatus == MOTOR_ERROR) {
        Serial.println("❌ 系统状态: 错误状态");
    } else {
        Serial.println("⏸️  系统状态: 待机中");
    }
    
    Serial.println("========================");
}

// ==================== 标定功能（从stepper_controller移植） ====================

void MotionController::setThetaStepsPerDegree(float steps) {
    thetaStepsPerDegree = steps;
    Serial.printf("📐 角度轴标定值设置为: %.3f步/度\n", steps);
}

void MotionController::setRhoStepsPerMM(float steps) {
    rhoStepsPerMM = steps;
    Serial.printf("📏 半径轴标定值设置为: %.3f步/mm\n", steps);
}

float MotionController::getThetaStepsPerDegree() const {
    return thetaStepsPerDegree;
}

float MotionController::getRhoStepsPerMM() const {
    return rhoStepsPerMM;
}

void MotionController::printCalibrationInfo() {
    Serial.println("=== 当前标定参数 ===");
    Serial.printf("📐 角度轴: %.3f步/度\n", thetaStepsPerDegree);
    Serial.printf("   一圈(360度): %.0f步\n", thetaStepsPerDegree * 360.0);
    Serial.println();
    Serial.printf("📏 半径轴: %.3f步/mm\n", rhoStepsPerMM);
    Serial.printf("   移动10mm: %.0f步\n", rhoStepsPerMM * 10.0);
    Serial.println("=================");
}

void MotionController::updateCalibrationFromStructure() {
    // 🔧 基于结构参数动态计算标定值
    
    // 角度轴标定：使用rotsteps命令设置的ROTATION_STEPS_PER_REV
    // 直接从当前设置的每圈步数计算步/度
    thetaStepsPerDegree = (float)ROTATION_STEPS_PER_REV / 360.0;
    
    // 半径轴标定：使用radsteps命令设置的RADIUS_MAX_STEPS  
    // 假设RADIUS_MAX_STEPS对应110mm行程（基于你的实际机械结构）
    rhoStepsPerMM = (float)RADIUS_MAX_STEPS / 110.0;
    
    // 同步更新补偿参数
    radiusCompensationRatio = COMPENSATION_FACTOR;
    
    Serial.println("✅ 标定值已根据当前参数更新:");
    Serial.printf("   角度轴: %d步/圈 → %.3f步/度\n", ROTATION_STEPS_PER_REV, thetaStepsPerDegree);
    Serial.printf("   半径轴: %d步/110mm → %.3f步/mm\n", RADIUS_MAX_STEPS, rhoStepsPerMM);
    Serial.printf("   补偿系数: %.3f (绘图和归零统一)\n", COMPENSATION_FACTOR);
}

void MotionController::resetCalibration() {
    // 🔧 修改：使用结构参数计算，而非硬编码值
    updateCalibrationFromStructure();
    Serial.println("🔄 标定参数已重置为结构参数计算值");
}

void MotionController::rotateByDegrees(float degrees) {
    Serial.printf("🔧 开始标定转动: %.1f度\n", degrees);
    
    if (!motorsEnabled) {
        Serial.println("📝 电机未启用，正在启用...");
        enableMotors();
        delay(100);
    }
    
    long steps = (long)(degrees * thetaStepsPerDegree);
    Serial.printf("🔄 计算结果: %.1f度 = %ld步\n", degrees, steps);
    
    // 使用标定专用速度
    int calibrationSpeed = 80;
    rotationMotor->setMaxSpeed(calibrationSpeed);
    rotationMotor->setAcceleration(40);
    
    long startPos = rotationMotor->currentPosition();
    long targetPos = startPos + steps;
    rotationMotor->moveTo(targetPos);
    
    // 非阻塞运行 - 移除超时保护，直到移动完成
    while (rotationMotor->distanceToGo() != 0) {
        rotationMotor->run();
        yield();
    }
    
    updateMotorActivity();
    Serial.println("✅ 转动完成");
    
    // 恢复正常速度
    configure28BYJ48();
}

void MotionController::moveByDistance(float mm) {
    Serial.printf("🔧 开始标定移动: %.1fmm\n", mm);
    
    if (!motorsEnabled) {
        Serial.println("📝 电机未启用，正在启用...");
        enableMotors();
        delay(100);
    }
    
    long steps = (long)(mm * rhoStepsPerMM);
    Serial.printf("📐 计算结果: %.1fmm = %ld步\n", mm, steps);
    
    // 使用标定专用速度
    int calibrationSpeed = 120;
    radiusMotor->setMaxSpeed(calibrationSpeed);
    radiusMotor->setAcceleration(60);
    
    long startPos = radiusMotor->currentPosition();
    long targetPos = startPos + steps;
    radiusMotor->moveTo(targetPos);
    
    // 非阻塞运行 - 移除超时保护，直到移动完成
    while (radiusMotor->distanceToGo() != 0) {
        radiusMotor->run();
        yield();
    }
    
    updateMotorActivity();
    Serial.println("✅ 移动完成");
    
    // 恢复正常速度
    configure28BYJ48();
}

// ==================== 半径轴补偿功能（从stepper_controller移植） ====================

void MotionController::enableRadiusCompensation(bool enable) {
    radiusCompensationEnabled = enable;
    if (enable) {
        Serial.printf("✅ 半径轴补偿已启用 - 补偿比例: %.3f\n", radiusCompensationRatio);
    } else {
        Serial.println("❌ 半径轴补偿已禁用");
    }
}

void MotionController::setRadiusCompensationRatio(float ratio) {
    radiusCompensationRatio = ratio;
    COMPENSATION_FACTOR = ratio;  // 🔧 同步更新主补偿参数
    Serial.printf("⚙️  补偿比例设置为: %.3f (绘图和归零统一)\n", ratio);
}

void MotionController::calculateRadiusCompensationForHoming(long thetaDelta, long& rhoCompensation) {
    if (!radiusCompensationEnabled) {
        rhoCompensation = 0;
        return;
    }
    
    // 计算半径轴补偿量（归零专用，配合机械耦合）
    rhoCompensation = -(long)(thetaDelta * radiusCompensationRatio);
    
    float thetaDegrees = (float)thetaDelta / thetaStepsPerDegree;
    float rhoMM = (float)abs(rhoCompensation) / rhoStepsPerMM;
    
    Serial.printf("🔧 归零补偿计算: θ变化=%.1f度(%ld步) → ρ补偿=%.1fmm(%ld步)\n", 
        thetaDegrees, thetaDelta, rhoMM, rhoCompensation);
}

// ==================== 归零功能（从stepper_controller移植） ====================

bool MotionController::isHomed() {
    return !isHoming && (thetaStatus != MOTOR_ERROR) && (rhoStatus != MOTOR_ERROR);
}

bool MotionController::isHomingInProgress() {
    return isHoming;
}

void MotionController::setHomingSpeed(float stepsPerSecond) {
    HOMING_SPEED_SPS = (int)stepsPerSecond;
    Serial.printf("🏠 归零速度设置为: %.0f步/秒\n", stepsPerSecond);
}

bool MotionController::homeMotorsWithCompensation() {
    Serial.println("🏠 MotionController统一归零开始...");
    
    // 检查限位开关引脚是否已配置
    if (thetaHomePin < 0 || rhoHomePin < 0) {
        Serial.println("❌ 限位开关引脚未配置，无法执行归零");
        return false;
    }
    
    // 安全检查
    if (isHoming) {
        Serial.println("⚠️  系统已在归零过程中，忽略重复请求");
        return false;
    }
    
    Serial.println("🔍 归零操作安全检查通过");
    Serial.println("🏠 开始MotionController统一智能归零...");
    
    // 设置归零状态
    isHoming = true;
    thetaStatus = MOTOR_HOMING;
    rhoStatus = MOTOR_HOMING;
    
    // 更新电机活动并启用电机
    updateMotorActivity();
    enableMotors();
    
    // 配置限位开关引脚
    pinMode(thetaHomePin, INPUT_PULLUP);
    pinMode(rhoHomePin, INPUT_PULLUP);
    
    // 🚀 使用统一的高速配置进行归零（与绘图完全统一）
    rotationMotor->setMaxSpeed(MAX_SPEED_SPS);    // 使用统一的速度参数
    radiusMotor->setMaxSpeed(MAX_SPEED_SPS);      // 使用统一的速度参数
    rotationMotor->setAcceleration(MAX_SPEED_SPS);
    radiusMotor->setAcceleration(MAX_SPEED_SPS);
    
    Serial.printf("⚙️  归零配置: %d步/秒 (与start/move/home完全统一)\n", MAX_SPEED_SPS);
    Serial.printf("🔍 当前电机实际速度: 旋转=%.0f步/秒, 半径=%.0f步/秒\n", 
                  rotationMotor->maxSpeed(), radiusMotor->maxSpeed());
    
    // 记录初始位置
    long initialThetaPos = rotationMotor->currentPosition();
    long initialRhoPos = radiusMotor->currentPosition();
    
    // 检查限位开关初始状态
    bool thetaAtHome = (digitalRead(thetaHomePin) == LOW);
    bool rhoAtHome = (digitalRead(rhoHomePin) == LOW);
    
    Serial.printf("🔍 限位开关状态: θ=%s, ρ=%s\n", 
        thetaAtHome ? "已触发" : "未触发",
        rhoAtHome ? "已触发" : "未触发");
    
    // 阶段1: 角度轴回零（带半径轴实时同步补偿）
    if (!thetaAtHome) {
        Serial.println("🔄 阶段1: 角度轴与半径轴实时同步回零");
        
        // 计算回零距离
        float maxHomingDegrees = 720.0;  // 🔧 修改为2圈，减少归零时间
        long thetaTargetDistance = -(long)(maxHomingDegrees * thetaStepsPerDegree);
        long totalRhoCompensation = 0;
        
        calculateRadiusCompensationForHoming(thetaTargetDistance, totalRhoCompensation);
        
        // 简单步进回零
        int stepCount = 0;
        unsigned long homeStartTime = millis();
        unsigned long maxHomingTime = 180000;  // 3分钟超时
        long maxStepCount = abs(thetaTargetDistance);
        
        while (digitalRead(thetaHomePin) == HIGH && stepCount < maxStepCount && (millis() - homeStartTime) < g_config.homingTimeout) {
            // 🔧 添加调试：每次循环检查限位开关状态
            bool thetaSwitchState = (digitalRead(thetaHomePin) == LOW);
            if (stepCount % (g_config.homingStepSize * 10) == 0) {  // 每200步输出一次
                Serial.printf("🔍 归零进度: %d步, 限位开关=%s, 剩余时间=%.1fs\n", 
                             stepCount, thetaSwitchState ? "触发" : "未触发",
                             (g_config.homingTimeout - (millis() - homeStartTime)) / 1000.0);
            }
            
            // 同步移动：角度轴+半径轴补偿
            rotationMotor->move(-g_config.homingStepSize);
            
            long rhoCompensationSteps = 0;
            calculateRadiusCompensationForHoming(-g_config.homingStepSize, rhoCompensationSteps);
            
            if (abs(rhoCompensationSteps) > 0) {
                radiusMotor->move(rhoCompensationSteps);
            }
            
            // 🔧 优化：提取电机同步运行逻辑
            runMotorsUntilComplete();
            
            stepCount += g_config.homingStepSize;
            
            // 🔧 安全检查：防止死循环
            if (stepCount > maxStepCount * 1.2) {  // 超过120%预期步数时强制退出
                Serial.println("⚠️ 角度轴归零步数异常，强制退出防止死循环");
                break;
            }
            
            yield();
        }
        
        rotationMotor->stop();
        radiusMotor->stop();
        
        if (digitalRead(thetaHomePin) == LOW) {
            Serial.println("✅ 角度轴已到达零位");
            rotationMotor->setCurrentPosition(0);
        } else {
            Serial.println("⚠️ 角度轴强制归零");
            rotationMotor->setCurrentPosition(0);
        }
    } else {
        Serial.println("✅ 角度轴已在零位");
        rotationMotor->setCurrentPosition(0);
    }
    
    // 阶段2: 半径轴回零 - 🚀 使用与角度轴相同的高效批量移动方式
    if (!rhoAtHome || digitalRead(rhoHomePin) == HIGH) {
        Serial.println("🔄 阶段2: 半径轴高速批量回零");
        
        radiusMotor->setMaxSpeed(MAX_SPEED_SPS);  // 🚀 使用统一的高速配置
        radiusMotor->setAcceleration(MAX_SPEED_SPS);  // 使用统一的加速度
        
        Serial.printf("⚙️  半径轴回零配置: %d步/秒 (与角度轴完全一致)\n", MAX_SPEED_SPS);
        
        float maxHomingDistance = 120.0;  // mm
        long maxRhoSteps = (long)(maxHomingDistance * rhoStepsPerMM);
        
        int stepCount = 0;
        unsigned long rhoHomeStartTime = millis();
        unsigned long maxRhoHomingTime = 120000;  // 2分钟超时（比角度轴稍短）
        
        while (digitalRead(rhoHomePin) == HIGH && stepCount < maxRhoSteps && (millis() - rhoHomeStartTime) < g_config.homingTimeout/1.5) {
            // 批量移动
            radiusMotor->move(-g_config.homingStepSize);
            
            // 🔧 优化：使用统一的电机运行函数
            runSingleMotorUntilComplete(radiusMotor, "半径轴");
            
            stepCount += g_config.homingStepSize;
            
            // 🔧 优化：减少进度输出频率
            if (stepCount % (g_config.homingStepSize * 5) == 0) {
                Serial.printf("⏳ 半径轴归零: %d步\n", stepCount);
            }
        }
        
        radiusMotor->stop();
        
        if (digitalRead(rhoHomePin) == LOW) {
            Serial.println("✅ 半径轴已到达零位");
            radiusMotor->setCurrentPosition(0);
        } else {
            Serial.println("⚠️ 半径轴强制归零");
            radiusMotor->setCurrentPosition(0);
        }
    } else {
        Serial.println("✅ 半径轴已在零位");
        radiusMotor->setCurrentPosition(0);
    }
    
    // 完成归零
    Serial.println("🎉 MotionController统一归零完成!");
    
    // 重置状态
    isHoming = false;
    thetaStatus = MOTOR_IDLE;
    rhoStatus = MOTOR_IDLE;
    
    // 强制重置绘图逻辑位置，确保与电机物理位置同步
    currentPosition = {0, 0, 0, 0};
    targetPosition = {0, 0, 0, 0};
    resetCompensation();
    
    // 恢复正常速度配置
    configure28BYJ48();
    
    Serial.printf("✨ 最终位置: θ=%ld, ρ=%ld\n", 
        rotationMotor->currentPosition(), radiusMotor->currentPosition());
    
    return true;
}

// ==================== 基本移动功能（从StepperController整合） ====================

void MotionController::moveTheta(int steps) {
    if (!motorsEnabled) enableMotors();
    updateMotorActivity();
    rotationMotor->move(steps);
    thetaStatus = MOTOR_MOVING;
    Serial.printf("角度轴移动: %d步\n", steps);
}

void MotionController::moveRho(int steps) {
    if (!motorsEnabled) enableMotors();
    updateMotorActivity();
    radiusMotor->move(steps);
    rhoStatus = MOTOR_MOVING;
    Serial.printf("半径轴移动: %d步\n", steps);
}

void MotionController::forceSetPosition(long thetaPos, long rhoPos) {
    rotationMotor->setCurrentPosition(thetaPos);
    radiusMotor->setCurrentPosition(rhoPos);
    Serial.printf("强制设置位置: θ=%ld, ρ=%ld\n", thetaPos, rhoPos);
}

void MotionController::resetPosition() {
    rotationMotor->setCurrentPosition(0);
    radiusMotor->setCurrentPosition(0);
    // 同步重置内部位置
    currentPosition = {0, 0, 0, 0};
    targetPosition = {0, 0, 0, 0};
    Serial.println("位置已重置为零");
}

long MotionController::getThetaPosition() { 
    return rotationMotor->currentPosition(); 
}

long MotionController::getRhoPosition() { 
    return radiusMotor->currentPosition(); 
}

void MotionController::getCurrentCoords(float& theta, float& rho) {
    long thetaPos = rotationMotor->currentPosition();
    long rhoPos = radiusMotor->currentPosition();
    
    theta = (float)thetaPos / thetaStepsPerDegree;
    rho = (float)rhoPos / rhoStepsPerMM;
}

void MotionController::printCurrentPosition() {
    Serial.printf("当前位置: θ=%ld步, ρ=%ld步\n", 
        rotationMotor->currentPosition(), radiusMotor->currentPosition());
}

bool MotionController::selfTest() {
    Serial.println("🔧 MotionController自检开始...");
    
    if (!motorsEnabled) {
        enableMotors();
        delay(100);
    }
    
    // 测试角度轴
    Serial.println("测试角度轴...");
    rotationMotor->move(100);
    while (rotationMotor->distanceToGo() != 0) {
        rotationMotor->run();
        yield();
    }
    delay(500);
    rotationMotor->move(-100);
    while (rotationMotor->distanceToGo() != 0) {
        rotationMotor->run();
        yield();
    }
    
    // 测试半径轴
    Serial.println("测试半径轴...");
    radiusMotor->move(100);
    while (radiusMotor->distanceToGo() != 0) {
        radiusMotor->run();
        yield();
    }
    delay(500);
    radiusMotor->move(-100);
    while (radiusMotor->distanceToGo() != 0) {
        radiusMotor->run();
        yield();
    }
    
    Serial.println("✅ MotionController自检完成");
    return true;
}

// 向后兼容的参数设置函数（弃用提示）
void MotionController::setDrawingSpeed(float speed) {
    Serial.printf("⚠️ setDrawingSpeed已弃用，请使用setMaxSpeed()\n");
    Serial.printf("   当前统一速度: %d步/秒\n", MAX_SPEED_SPS);
}

void MotionController::setStepsPerRevolution(int steps) {
    ROTATION_STEPS_PER_REV = steps;
    Serial.printf("每圈步数设置: %d步\n", steps);
}

void MotionController::setMaxRadius(float radius) {
    float maxRhoSteps = radius * rhoStepsPerMM;
    RADIUS_MAX_STEPS = (int)maxRhoSteps;
    Serial.printf("最大半径设置: %.1fmm (%d步)\n", radius, RADIUS_MAX_STEPS);
}

// ==================== 运行控制（从StepperController整合） ====================

void MotionController::run() {
    rotationMotor->run();
    radiusMotor->run();
}

// ==================== 🔧 优化：提取的辅助函数 ====================

void MotionController::runMotorsUntilComplete() {
    static unsigned long lastLEDUpdate = 0;
    static unsigned long lastDebugOutput = 0;
    
    long initialThetaDistance = rotationMotor->distanceToGo();
    long initialRhoDistance = radiusMotor->distanceToGo();
    
    // 🔧 添加调试：只有在有移动时才输出开始信息
    if (initialThetaDistance != 0 || initialRhoDistance != 0) {
        Serial.printf("🔄 电机同步运行: θ=%ld步, ρ=%ld步\n", initialThetaDistance, initialRhoDistance);
    }
    
    while (rotationMotor->distanceToGo() != 0 || radiusMotor->distanceToGo() != 0) {
        rotationMotor->run();
        radiusMotor->run();
        
        // LED更新优化
        if (ledControlCenter != nullptr && (millis() - lastLEDUpdate > MotionConstants::LED_UPDATE_INTERVAL)) {
            ledControlCenter->update();
            lastLEDUpdate = millis();
        }
        
        // 🔧 添加调试：每2秒输出一次进度（仅在归零时），减少串口输出
        // if (isHoming && (millis() - lastDebugOutput > 2000)) {
        //     long thetaRemaining = rotationMotor->distanceToGo();
        //     long rhoRemaining = radiusMotor->distanceToGo();
        //     if (thetaRemaining != 0 || rhoRemaining != 0) {
        //         Serial.printf("⏳ 电机运行中: θ剩余=%ld步, ρ剩余=%ld步\n", thetaRemaining, rhoRemaining);
        //     }
        //     lastDebugOutput = millis();
        // }
        
        yield();
    }
    
    // 🔧 添加调试：完成信息
    if (initialThetaDistance != 0 || initialRhoDistance != 0) {
        Serial.println("✅ 电机同步运行完成");
    }
}

void MotionController::runSingleMotorUntilComplete(AccelStepper* motor, const char* motorName) {
    static unsigned long lastLEDUpdate = 0;
    
    while (motor->distanceToGo() != 0) {
        motor->run();
        
        // LED更新优化
        if (ledControlCenter != nullptr && (millis() - lastLEDUpdate > MotionConstants::LED_UPDATE_INTERVAL)) {
            ledControlCenter->update();
            lastLEDUpdate = millis();
        }
        
        yield();
    }
}

void MotionController::configureMotorSpeed(AccelStepper* motor, int speed) {
    motor->setMaxSpeed(speed);
    motor->setAcceleration(speed);
}

// 静态函数已在头文件中内联定义，无需在此重复实现

// ==================== 扫图功能 ====================

bool MotionController::executeSweepPattern() {
    Serial.println("🔄 生成12圈螺旋清理扫图极坐标数组");
    
    if (!motorsEnabled) {
        enableMotors();
        delay(100);
    }
    
    // 🔧 重置扫图暂停标志，开始扫图
    sweepPaused = false;
    
    // 🌀 生成12圈螺旋清理极坐标点数组
    const int totalCircles = 12;
    const int pointsPerCircle = 6;  // 每圈6个点，更平滑
    const int totalPoints = totalCircles * pointsPerCircle;
    
    Serial.printf("🌀 生成扫图路径：%d圈 × %d点/圈 = %d个极坐标点\n", 
                  totalCircles, pointsPerCircle, totalPoints);
    
    // 动态分配极坐标数组
    struct SweepPoint {
        float theta;
        float rho;
    };
    
    SweepPoint* sweepPoints = new SweepPoint[totalPoints];
    
    // 生成螺旋清理路径：从外圈到圆心
    float currentTheta = 0.0;
    float startRadius = 1.0;    // 外圈（110mm）
    float endRadius = 0.0;      // 圆心
    float radiusStep = (startRadius - endRadius) / totalPoints;
    float angleStep = (2.0 * PI) / pointsPerCircle;  // 每圈内的角度步长
    
    for (int i = 0; i < totalPoints; i++) {
        sweepPoints[i].theta = currentTheta;
        sweepPoints[i].rho = startRadius - (i * radiusStep);
        
        // 确保半径不为负
        if (sweepPoints[i].rho < 0) sweepPoints[i].rho = 0;
        
        // 连续累积角度
        currentTheta += angleStep;
    }
    
    Serial.println("✅ 扫图极坐标数组生成完成，开始执行绘图逻辑");
    
    // 🎯 关键：直接使用现有绘图逻辑执行扫图
    for (int i = 0; i < totalPoints; i++) {
        // 检查暂停标志
        if (sweepPaused) {
            Serial.println("⏸️ 扫图已暂停");
            delete[] sweepPoints;
            return false;
        }
        
        // 调用现有的moveTo()函数
        moveTo(sweepPoints[i].theta, sweepPoints[i].rho);
        
        // 等待移动完成
        while(isMovingNow()) { 
            update(); 
            if(sweepPaused) {
                delete[] sweepPoints;
                return false;
            }
            yield(); 
        }
        
        // 每圈输出一次进度
        if ((i + 1) % pointsPerCircle == 0) {
            int currentCircle = ((i + 1) / pointsPerCircle);
            Serial.printf("🔄 第%d圈完成 (ρ=%.3f)\n", 
                          currentCircle, sweepPoints[i].rho);
        }
    }
    
    // 清理内存
    delete[] sweepPoints;
    
    Serial.println("🧹 12圈螺旋扫图完成！使用现有绘图系统确保坐标准确");
    Serial.printf("📍 扫图结束位置: θ=%.3f弧度, ρ=%.3f（圆心）\n", 
                  currentPosition.theta, currentPosition.rho);
    
    return true;
}

bool MotionController::executeSweepAndHome() {
    Serial.println("🧹� 开始执行完整扫图流程：归零 → 扫图 → 就绪");

    
    // 第一步：归零操作
    Serial.println("📍 第1步：执行初始归零");
    if (!homeMotorsWithCompensation()) {
        Serial.println("❌ 初始归零失败，取消扫图操作");
        return false;
    }
    
    delay(500); // 归零后短暂停顿
    
    // 第二步：执行扫图
    Serial.println("📍 第2步：执行扫图整理");
    if (!executeSweepPattern()) {
        Serial.println("❌ 扫图操作失败");
        return false;
    }
    
    delay(1000); // 扫图后停顿，让效果稳定
    
    // 🔧 修复：扫图完成后不再归零，直接准备就绪状态
    Serial.println("📍 第3步：扫图完成，准备就绪");
    Serial.println("🎉 完整扫图流程完成！");
    Serial.println("   ✅ 沙盘表面已整理完成");
    Serial.println("   ✅ 设备准备就绪，等待开始绘图");
    Serial.println("   💡 下次start命令将从归零开始正常绘图流程");
    
    return true;
}
